#include "WProgram.h"
#include "DiffDriveBot.h"

DiffDriveBot::DiffDriveBot(int rf, int rb, int lf, int lb){
	_rf = rf;
	_rb = rb;
	_lf = lf;
	_lb = lb;
}

void DiffDriveBot::drive(char direction, int speed){
	CServo::CServo rf(_rf);
	CServo::CServo rb(_rb);
	CServo::CServo lf(_lf);
	CServo::CServo lb(_lb);
	switch(direction){
	case 'f':
		lf.writeSpeed(2, speed);
		lb.writeSpeed(2, speed);
		rf.writeSpeed(1, speed);
		rb.writeSpeed(1, speed);
		break;
	case 'b':
		lf.writeSpeed(1, speed);
		lb.writeSpeed(1, speed);
		rf.writeSpeed(2, speed);
		rb.writeSpeed(2, speed);
		break;
	}
}

void DiffDriveBot::stop(){
	CServo::CServo rf(_rf);
	CServo::CServo rb(_rb);
	CServo::CServo lf(_lf);
	CServo::CServo lb(_lb);
	lf.stop();
	lb.stop();
	rf.stop();
	rb.stop();
}

void DiffDriveBot::setLeftRightSpeedPerc(int left, int right){
	CServo::CServo rf(_rf);
	CServo::CServo rb(_rb);
	CServo::CServo lf(_lf);
	CServo::CServo lb(_lb);
	negleft = left*-1;
	negright - right*-1;
	if (left<0){
		lf.writeSpeed(1, negleft);
		lb.writeSpeed(1, negleft);
	}
	if (left>0){
		lf.writeSpeed(2, left);
		lb.writeSpeed(2, left);
	}
	if (right<0){
		rf.writeSpeed(2, negright);
		rb.writeSpeed(2, negright);
	}
	if (right>0){
		rf.writeSpeed(1, right);
		rb.writeSpeed(1, right);
	}
}

//END OMNIBOT\\

//BEGIN CSERVO\\


CServo::CServo(int pin){
        _spin = pin;
        pinMode(_spin, OUTPUT);
}
void CServo::writeSpeed(int direction /*1=clockwise, 2=counterclockwise*/, int speedpercent /*0-100*/){
        if (direction == 1){
                clockwise_speed = ((speedpercent*-3.3)+1500);
                digitalWrite(_spin, HIGH);
                delayMicroseconds(clockwise_speed);
                digitalWrite(_spin, LOW);
                delay(20);
        }
        if (direction == 2){
                cclockwise_speed = ((speedpercent*2.7)+1500);
                digitalWrite(_spin, HIGH);
                delayMicroseconds(cclockwise_speed);
                digitalWrite(_spin, LOW);
                delay(20);
        }
}
void CServo::stop(){
        digitalWrite(_spin, HIGH);
        delayMicroseconds(1500);
        digitalWrite(_spin, LOW);
        delay(20);
}

